#include "d_Math.h"
#include <iostream>
#include <assert.h>

using namespace std;

using namespace d_Math;

bool d_Math::equal_to_zero(float x)
{
	if(x<0.000001&&x>-0.000001)
		return true;
	return false;
}
/*
bool checkPointInTriangle(d_Vec3f point, d_Vec3f pa, d_Vec3f pb, d_Vec3f pc)
{
  d_Vec3f e10=pb-pa;
  d_Vec3f e20=pc-pa;
  float a = dot(e10,e10);
  float b = dot(e10,e20);
  float c = dot(e20,e20);
  float ac_bb=(a*c)-(b*b);
  d_Vec3f vp(point[0]-pa[0], point[1]-pa[1], point[2]-pa[2]);
  float d = dot(vp,e10);
  float e = dot(vp,e20);
  float x = (d*c)-(e*b);
  float y = (e*a)-(d*b);
  float z = x+y-ac_bb;
  return (( in(z)& ~(in(x)|in(y)) ) & 0x80000000);
}
*/
d_Vecf d_Math::solve_tridiagonal_system(d_Matf mat, d_Vecf b)
{
	d_Vecf result;
	result.set_size(b.get_size());

	for(int i=1; i<b.get_size(); i++)
	{
		if(mat(i-1,i-1)==0)
			return result;
		float m = mat(i,i-1)/mat(i-1,i-1);
		mat(i,i) = mat(i,i) - m*mat(i-1,i);
		b[i] = b[i] - m*b[i-1];
	}
	if(mat(b.get_size()-1,b.get_size()-1)==0)
		return result;
	result[b.get_size()-1] = b[b.get_size()-1] / mat(b.get_size()-1,b.get_size()-1);

	for(int i=b.get_size()-2; i>=0; i--)
		result[i] = (b[i]-mat(i,i+1)*result[i+1])/mat(i,i);

	return result;
}

d_Vecf d_Math::solve_system_sor(d_Matf mat,d_Vecf b,float w)
{
	d_Vecf phi;
	phi.set_size(b.get_size());

	for(int i=0; i<phi.get_size(); i++)
		phi[i]=1;

	bool converged = false;

	for(int k=0; k<10; k++)
	{
		for(int i=0; i<mat.m; i++)
		{
			double sigma = 0;
			for(int j=0; j<mat.n; j++)
				if(i!=j)
					sigma = sigma + mat(i,j)*phi[j];
			//phi[i] = (b[i]-sigma)/mat(i,i);
			phi[i] = (1-w)*phi[i]+(w/mat(i,i))*(b[i]-sigma);
		}
	}
	

	return phi;
}

d_Vecf d_Math::solve_system_cg(d_Matf A, d_Vecf b)
{
	A.print();
	b.print();

	d_Vecf x, d, r, rold;
	float a, beta;
	x.set_size(b.get_size());
	for(int i=0; i<b.get_size(); i++)
		x[i]=1;
	d.set_size(b.get_size());
	r.set_size(b.get_size());
	rold.set_size(b.get_size());

	d = r = b - A*x;

	d.print();
	x.print();
	
	//for(int i=0; i<10; i++)
	int count =0;
	while(r*r<b*b*0.00001&&count<10)
	{
		
		a = (r*r)/(d*A*d);
		x = x + d*a;
		for(int k=0; k<r.get_size(); k++)
			rold[k] = r[k];
		r = r - A*d*a;
		beta = (r*r)/(rold*rold);
		d = r + d*beta;
		count++;
	}
	//std::cout << "Count: " << count << std::endl;
	return x;
}

d_Mat4f d_Math::HScale4f(d_Vec3f x)
{
	d_Mat4f tmp;
	tmp(0,0) = x[0];
	tmp(1,1) = x[1];
	tmp(2,2) = x[2];
	tmp(3,3) = 1;
	return tmp;
}

d_Mat4f d_Math::HScale4f(float x, float y, float z)
{
	d_Mat4f tmp;
	tmp(0,0) = x;
	tmp(1,1) = y;
	tmp(2,2) = z;
	tmp(3,3) = 1;
	return tmp;
}

d_Mat4f d_Math::HRot4f(d_Vec3f v, float a)
{
	v = norm(v);
	float c = cos(a), s = sin(a);
	d_Mat4f tmp(
	            v[0]*v[0]+(1-v[0]*v[0])*s, v[0]*v[1]*(1-c)-v[2]*s, v[0]*v[2]*(1-c)+v[1]*s,0,
	            v[0]*v[1]*(1-c)+v[2]*s, v[1]*v[1]+(1-v[1]*v[1])*c, v[1]*v[2]*(1-c)-v[0]*s,0,
	            v[0]*v[2]*(1-c)-v[1]*s, v[1]*v[2]*(1-c)+v[0]*s, v[0]*v[0]+(1-v[2]*v[2])*c,0,
	            0,0,0,1);
	
	
	//tmp.diagonal(1);
	return tmp;
}

d_Mat4f d_Math::HXRot4f(float a)
{
	float c = cos(a), s = sin(a);
	d_Mat4f tmp(1,0,0,0,
	            0,c,-s,0,
	            0,s,c,0,
	            0,0,0,1);
	return tmp;
}

d_Mat4f d_Math::HYRot4f(float a)
{
	float c = cos(a), s = sin(a);
	d_Mat4f tmp(c,0,s,0,
	            0,1,0,0,
	            -s,0,c,0,
	            0,0,0,1);
	return tmp;
}

d_Mat4f d_Math::HZRot4f(float a)
{
	float c = cos(a), s = sin(a);
	d_Mat4f tmp(c,-s,0,0,
	            s,c,0,0,
	            0,0,1,0,
	            0,0,0,1);
	
	
	//tmp.diagonal(1);
	return tmp;
}

d_Mat4f d_Math::HTrans4f(d_Vec3f t)
{
	d_Mat4f tmp;
	tmp.diagonal(1);
	tmp(0,3)=t[0];
	tmp(1,3)=t[1];
	tmp(2,3)=t[2];
	return tmp;
}

d_Mat3f d_Math::adj(const d_Mat3f &m)			
{
	d_Mat3f	result;
	
	d_Vec3f tmp0(m(0,0),m(0,1),m(0,2));
	d_Vec3f tmp1(m(1,0),m(1,1),m(1,2));
	d_Vec3f tmp2(m(2,0),m(2,1),m(2,2));
	d_Vec3f res0 = cross(tmp1, tmp2);
	d_Vec3f res1 = cross(tmp2, tmp0);
	d_Vec3f res2 = cross(tmp0, tmp1);
	
	for(int i=0; i<3;i++)
	{
		result(0,i) = res0[i];
		result(1,i) = res1[i];
		result(2,i) = res2[i];
	}

	return(result);
}

d_Mat3f d_Math::trans(const d_Mat3f &m)
{
	d_Mat3f res;
	for(int i=0; i<3; i++)
		for(int j=0; j<3; j++)
			res(i,j) = m(j,i);
	return res;
}

d_Mat3f d_Math::inv(const d_Mat3f &m)
{
	float	mDet;
	d_Mat3f	adjoint;
	d_Mat3f	result;
	
	adjoint = adj(m);

	d_Vec3f tmp1(adjoint(0,0), adjoint(0,1), adjoint(0,2));
	d_Vec3f tmp2(m(0,0),m(0,1),m(0,2));
	mDet = dot(tmp1, tmp2);
	
	result = trans(adjoint);
	result = result/mDet;
	
	return(result);
}

d_Mat4f d_Math::adj(const d_Mat4f &m)			
{
	d_Mat4f	result;

	d_Vec4f tmp0(m(0,0),m(0,1),m(0,2),m(0,3));
	d_Vec4f tmp1(m(1,0),m(1,1),m(1,2),m(1,3));
	d_Vec4f tmp2(m(2,0),m(2,1),m(2,2),m(2,3));
	d_Vec4f tmp3(m(3,0),m(3,1),m(3,2),m(3,3));
	d_Vec4f res0 = cross(tmp1, tmp2, tmp3);
	d_Vec4f res1 = cross(tmp0, tmp2, tmp3)*(-1);
	d_Vec4f res2 = cross(tmp0, tmp1, tmp3);
	d_Vec4f res3 = cross(tmp0, tmp1, tmp2)*(-1);
	
	for(int i=0; i<4;i++)
	{
		result(0,i) = res0[i];
		result(1,i) = res1[i];
		result(2,i) = res2[i];
		result(3,i) = res3[i];
	}

	return(result);
}

d_Mat4f d_Math::trans(const d_Mat4f &m)
{
	d_Mat4f res;
	for(int i=0; i<4; i++)
		for(int j=0; j<4; j++)
			res(i,j) = m(j,i);
	return res;
}

d_Mat4f d_Math::inv(const d_Mat4f &m)
{
	float	mDet;
	d_Mat4f	adjoint;
	d_Mat4f	result;
	
	adjoint = adj(m);

	d_Vec4f tmp1(adjoint(0,0), adjoint(0,1), adjoint(0,2),adjoint(0,3));
	d_Vec4f tmp2(m(0,0),m(0,1),m(0,2),m(0,3));
	mDet = dot(tmp1, tmp2);
	
	//assert(mDet != 0, "(d_Mat4::inv) matrix is non-singular");
	
	result = trans(adjoint);
	result = result/mDet;
	
	return(result);
}

d_Vec4f d_Math::cross(const d_Vec4f &a, const d_Vec4f &b, const d_Vec4f &c)	
{
	d_Vec4f result;
#define ROW(i)		 a[i], b[i], c[i]
#define DET(i,j,k)   dot(d_Vec3f(ROW(i)), cross(d_Vec3f(ROW(j)), d_Vec3f(ROW(k))))
	
	result[0] =  DET(1,2,3);
	result[1] = -DET(0,2,3);
	result[2] =  DET(0,1,3);
	result[3] = -DET(0,1,2);
	
	return(result);

#undef ROW
#undef DET
}




